{-# OPTIONS -XTypeFamilies -XTypeSynonymInstances -XFlexibleInstances -XUndecidableInstances #-}
{-# LANGUAGE ExistentialQuantification #-}

module Interpolation ( 
   lerp,
   biLerp,
   triLerp,
   lerpR,
   linearSampling,
 ) where

import Sim.Helper ( floor' )
import GHC.Float  ( int2Double, double2Int )

import Data.AdditiveGroup ( (^+^), (^-^) )
import Data.VectorSpace   ( VectorSpace, Scalar, (*^) )
import Data.Array.Unboxed


-- Linear interpolation
lerp :: (VectorSpace v, Double ~ Scalar v) => (Int -> v) -> (Double -> v)
lerp g x =  f ^+^ k *^ (f' ^-^ f)
  where
    f  = g $ x'
    f' = g $ x' + 1
    x' = floor' x
    k  = x - int2Double x'

-- Bilinear interpolation
biLerp f  = lerp . lerp f

-- Trilinear interpolation
triLerp f  = biLerp . lerp f

class Interpolation a where
  type R a :: *
  lerpR :: a -> R a

instance ( Int ~ a
         , Interpolation b , VectorSpace b
         , Double ~ (Scalar b) ) 
     => Interpolation ( a -> b ) where
   type R (a -> b) = Double -> R b
   lerpR g = lerpR.lerp g

instance Interpolation Double where
   type R Double = Double
   lerpR a = a

sampling :: (Double,Double) -> Double -> [(Double,Double)] -> [(Double,Double)]
sampling (l,r) h vs = loop [l,l+h .. r] $ zip vs $ tail vs where
  loop [] _ = []
  loop (x:xs) [] = (x,0):loop xs []
  loop (x:xs) v@((v0,v1):vs) | x < x0 = (x,0):loop xs v
                             | x > x1 = loop (x:xs) vs
                             | otherwise = (x,t) : loop xs v
    where (x0,y0) = v0
          (x1,y1) = v1
          t = y0 + (y1-y0)*(x-x0)/(x1-x0)

linearSampling :: (Double,Double) -> Double -> [(Double,Double)] -> (Double -> Double)
linearSampling (v0,v1) step l = (lerp f) . tr where
  tr x = (x - v0)/step
  f x | x < 0 || x > m = 0
      | otherwise = arr ! x
  m = floor' $ tr v1
  arr :: UArray Int Double
  arr = listArray (0,m) $ map snd $ sampling (v0,v1) step l